#include "ti_msp_dl_config.h"

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "mpu6050.h"
#include "mspm0_i2c.h"
#include "interrupt.h"

/* Data requested by client. */
#define PRINT_ACCEL     (0x01)
#define PRINT_GYRO      (0x02)
#define PRINT_QUAT      (0x04)

#define ACCEL_ON        (0x01)
#define GYRO_ON         (0x02)

#define MOTION          (0)
#define NO_MOTION       (1)

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (50)

#define FLASH_SIZE      (512)
#define FLASH_MEM_START ((void*)0x1800)

struct rx_s {
    unsigned char header[3];
    unsigned char cmd;
};

struct hal_s {
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro;
    unsigned short report;
    unsigned short dmp_features;
    unsigned char motion_int_mode;
    struct rx_s rx;
};

static struct hal_s hal = {0};

unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];

#define q30  (1073741824.0f) /* 2^30 = 1073741824 */
float pitch, roll, yaw;

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from thei
 * driver(s).
 * TODO: The following matrices refer to the configuration on an internal test
 * board at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};

static void tap_cb(unsigned char direction, unsigned char count)
{

}

static void android_orient_cb(unsigned char orientation)
{

}

/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
static inline unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

static inline unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}

void MPU6050_Init(void)
{
    int result; // 状态flag
    unsigned char accel_fsr; // 加速度计满量程范围
    unsigned short gyro_rate, gyro_fsr; // 采样率 陀螺仪满量程范围

    if(DL_I2C_getSDAStatus(I2C_MPU6050_INST) == DL_I2C_CONTROLLER_SDA_LOW){
        // 如果 I2C 总线的 SDA 引脚被拉低（可能被锁住），调用 mpu6050_i2c_sda_unlock() 尝试解锁 I2C 接口。
        mpu6050_i2c_sda_unlock();
    }

    result = mpu_init();
    if (result){
        // 返回值不为 0，说明初始化失败
        return;
    }

    result = 0; // 反复利用状态flag

    /* Get/set hardware configuration. Start gyro. */
    /* Wake up all sensors. */
    result += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 启用 XYZ 三轴陀螺仪和加速度计
    /* Push both gyro and accel data into the FIFO. */
    result += mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 将陀螺仪和加速度计的数据写入 FIFO
    result += mpu_set_sample_rate(DEFAULT_MPU_HZ); // 设置采样率
    /* Read back configuration in case it was set improperly. */
    result += mpu_get_sample_rate(&gyro_rate); // 读取当前陀螺仪采样率配置
    result += mpu_get_gyro_fsr(&gyro_fsr); // 读取当前陀螺仪满量程范围配置
    result += mpu_get_accel_fsr(&accel_fsr); // 读取当前陀螺仪加速度计满量程范围配置

    /* Initialize HAL state variables. */
    memset(&hal, 0, sizeof(hal)); // 初始化 hal 结构体
    hal.sensors = ACCEL_ON | GYRO_ON; // 设置当前启用的传感器为加速度计和陀螺仪
    hal.report = PRINT_QUAT; // 输出模式为输出四元数

    /* 初始化DMP:
     * 1. 调用dmp_load_motion_driver_firmware()。这会将DMP图像从inv_mpu_dmp_motion_driver.h推送到MPU内存中。
     * 2. 将陀螺仪和加速计方向矩阵推送至DMP。 
     * 3. 注册手势回调函数。别担心，除非对应的功能启用，否则这些回调函数不会被执行。 
     * 4. 调用dmp_enable_feature(mask)来启用不同的功能。
     * 5. 调用dmp_set_fifo_rate(freq) 来选择DMP输出速率。
     * 6. 调用任何特定功能的控制函数。
     *
     * 要启用DMP，只需调用mpu_set_dmp_state(1)。此函数可以重复调用以在运行时启用和禁用DMP。
     *
     * 以下是支持的DMP功能简短总结。 
     * inv_mpu_dmp_motion_driver.c 文件中提供的图像：
     * DMP_FEATURE_LP_QUAT: 在DMP上以200Hz的频率生成仅限陀螺仪的四元数。
     * 以更高的速率集成陀螺仪数据可以减少数值误差（与以较低的采样率在MCU上集成相比）。
     * DMP_FEATURE_6X_LP_QUAT: 在DMP上以200Hz的频率生成陀螺仪/加速度四元数。不能与DMP_FATURE_LP_QUAT结合使用。
     * DMP_FEATURE_TAP：检测X、Y和Z轴方向的轻拍。
     * DMP_FEATURE_ANDROID_ORIENT: 谷歌的屏幕旋转算法。在屏幕应旋转的四个方向上触发事件。
     * DMP_FEATURE_GYRO_CAL: 在八秒无运动后校准陀螺仪数据。
     * DMP_FEATURE_SEND_RAW_ACCEL: 将原始加速计数据添加到FIFO。
     * DMP_FEATURE_SEND_RAW_GYRO: A将原始陀螺仪数据添加到FIFO。
     * DMP_FEATURE_SEND_CAL_GYRO: 将校准的陀螺仪数据添加到FIFO。不能与DMP_FATURE_SEND_RAW_GYRO结合使用。
     */
     // DMP 是 MPU6050 内部的一个协处理器，可以用来进行姿态解算（如四元数计算）。
    result += dmp_load_motion_driver_firmware(); // 将 DMP 固件（在 inv_mpu_dmp_motion_driver.h 中定义）加载到 MPU6050 的内存中。

    // 将矩阵转换为标量形式供 DMP 使用。
    result += dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪的方向矩阵（orientation matrix），用于校正传感器的安装方向。
    
    // 注册回调函数,这些回调函数会在 DMP 检测到相关事件时被调用
    result += dmp_register_tap_cb(tap_cb); // 用于检测点击（tap）动作
    result += dmp_register_android_orient_cb(android_orient_cb); // 用于检测安卓屏幕方向变化

    // 设置 DMP 功能特性
    hal.dmp_features = 
                    DMP_FEATURE_6X_LP_QUAT |      // 启用六轴低功耗四元数解算
                    DMP_FEATURE_TAP |             // 启用点击检测
                    DMP_FEATURE_ANDROID_ORIENT |  // 启用安卓屏幕方向检测
                    DMP_FEATURE_SEND_RAW_ACCEL |  // 输出原始加速度数据
                    DMP_FEATURE_SEND_CAL_GYRO |   // 输出校准后的陀螺仪数据
                    DMP_FEATURE_GYRO_CAL;         // 启用陀螺仪自动校准

    // 启用 DMP 特性并设置 FIFO 输出频率
    result += dmp_enable_feature(hal.dmp_features); // 启用上面配置的 DMP 特性
    result += dmp_set_fifo_rate(DEFAULT_MPU_HZ);    // 设置 DMP 的 FIFO 输出频率
    result += mpu_set_dmp_state(1);               // 启动 DMP
    hal.dmp_on = 1;                                       // 记录 DMP 已启用

    if (result){ // 总错误大检查
        return;
    }


    /* Enable INT_GROUP1 handler. */
    enable_group1_irq = 1; // 启用中断处理
}

int Read_Quad(void)
{
    /* This function gets new data from the FIFO when the DMP is in
    * use. The FIFO can contain any combination of gyro, accel,
    * quaternion, and gesture data. The sensors parameter tells the
    * caller which data fields were actually populated with new data.
    * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
    * the FIFO isn't being filled with accel data.
    * The driver parses the gesture data to determine if a gesture
    * event has occurred; on an event, the application will be notified
    * via a callback (assuming that a callback function was properly
    * registered). The more parameter is non-zero if there are
    * leftover packets in the FIFO.
    */

    int result;

    do
    {
        result = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
    }while(more);

    if(result)
        return -1;

    float q0 = quat[0] / q30;
    float q1 = quat[1] / q30;
    float q2 = quat[2] / q30;
    float q3 = quat[3] / q30;

    pitch  = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
    roll   = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
    yaw    = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;

    return 0;
}